/*!
  @file image_publisher.h
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#ifndef _IMAGE_PUBLISHER_H_
#define _IMAGE_PUBLISHER_H_

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/core/core.hpp>

namespace vo {

/*! 
 *  @brief  Image publisher 
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 *  This class publishes images specified by parameter '~filename'
 */
class ImagePublisher {
 public:
  explicit ImagePublisher(const ros::NodeHandle &nh)
      : nh_(nh)
      , private_nh_("~")
      , it_(nh) 
  {
    // initializing pub/sub, server
    img_pub_ = it_.advertiseCamera(nh_.resolveName("image_out"), 1);
    imgL_pub_ = it_.advertiseCamera(nh_.resolveName("imageL_out"), 1);
    imgR_pub_ = it_.advertiseCamera(nh_.resolveName("imageR_out"), 1);

    // initializing optional parameters
    // e.g.) private_nh_.param<TYPE>("PARAM_NAME", PARAM_VAR, DEFAULT_VALUE);
    private_nh_.param<std::string>("filename", filename_, "pattern.png");
    private_nh_.param<std::string>("imageL_list", imgL_list_, "imgL");
    private_nh_.param<std::string>("imageR_list", imgR_list_, "imgR");
    private_nh_.param<double>("sim_rate", sim_rate_, 10.0);
  }

  ~ImagePublisher() { }

  void publishOneImage();

  void publishStereoImages();

 private:
  struct CameraIntParams {
    cv::Mat K;
    cv::Mat D;
  };

  ros::NodeHandle nh_;          //!< ROS node handler
  ros::NodeHandle private_nh_;  //!< ROS node handler in the private namespace
  image_transport::ImageTransport it_;
  image_transport::CameraPublisher img_pub_;
  image_transport::CameraPublisher imgL_pub_;
  image_transport::CameraPublisher imgR_pub_;

  std::string filename_;   //!< image file name
  std::string imgL_list_;  //!< image list name
  std::string imgR_list_;  //!< image list name

  double sim_rate_;  //!< simuration rate
};

}  // namespace vo

#endif  // _IMAGE_PUBLISHER_H_
